
package org.beltrw.navigation;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.util.Scanner;
import java.util.logging.Level;
import java.util.logging.Logger;
import lejos.pc.comm.NXTConnector;

/**
 * PC-side navigator for NXT
 * 
 * Reads in a line as a command in the form: "%f %f"
 * If it reads in "done", the program terminates
 *
 * The first number is used as a rotation angle
 * The second number is used as a distance to travel
 *
 * @author ryanbelt
 */
public class BTNavigator
{
    public static void main(String[] args)
    {
        NXTConnector conn = new NXTConnector();

        // Connect to any NXT over Bluetooth
        boolean connected = conn.connectTo();

        if (!connected)
        {
            System.err.println("Failed to connect to any NXT");
            System.exit(1);
        }

        DataOutputStream dos = conn.getDataOut();
        DataInputStream dis = conn.getDataIn();

        Scanner input = new Scanner(System.in);
        System.out.print("Command: ");
        String command = input.nextLine();

        while(!command.equalsIgnoreCase("done"))
        {
            try
            {
                Scanner scan = new Scanner(command);
                float num1 = scan.nextFloat();
                float num2 = scan.nextFloat();

                // push numbers to NXT
                dos.writeFloat(num1);
                dos.flush();
                dos.writeFloat(num2);
                dos.flush();

                System.out.println("Finished sending data\n");
                System.out.print("Command: ");
                command = input.nextLine();
            }
            catch (IOException ex)
            {
                System.err.println("An error occured:");
                System.err.println(ex.getMessage());
                ex.printStackTrace();
                Logger.getLogger(BTNavigator.class.getName()).log(Level.SEVERE, null, ex);
            }
        }

        System.out.println("Finished navigating robot");
    }

}
